# coding=utf-8
from naoqi import ALProxy
import time
import threading
import cmath

IP = "192.168.43.106"
PORT = 9559

def standUp():
    postureProxy.goToPosture("Stand", 0.5)
    pass

def catchTheCylinder():
    # 抓圆柱体(桌子高 37.4  据胸口10cm)
    motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RShoulderPitch", -8.1 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RElbowRoll", 2.6 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RElbowYaw", 0 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RHand", 1, 0.1)
    time.sleep(1)
    motionProxy.setAngles("RWristYaw", 82.5 * cmath.pi / 180, 0.2)
    time.sleep(1)
    motionProxy.setAngles("RShoulderRoll", 0 * cmath.pi / 180, 0.1)
    time.sleep(5)
    motionProxy.setAngles("RElbowRoll", 55 * cmath.pi / 180, 0.1)
    time.sleep(5)
    motionProxy.setAngles("RHand", 0, 0.05)
    time.sleep(5)

    # 放下手臂
    motionProxy.setAngles("RElbowRoll", 2.0 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -70 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderPitch", 80 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -17 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("LShoulderPitch", 111 * cmath.pi / 180, 0.1)
    motionProxy.setAngles("LShoulderRoll", 17 * cmath.pi / 180, 0.1)
    time.sleep(3)
    pass

def washFruit():
    # 洗水果
    motionProxy.setAngles("RShoulderRoll", -70 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderPitch", -10 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 3 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderRoll", 17 * cmath.pi / 180, 0.1)
    time.sleep(3)

    for i in range(2):
        motionProxy.setAngles("RElbowRoll", 50 * cmath.pi / 180, 0.1)
        time.sleep(2)
        motionProxy.setAngles("RElbowRoll", 2 * cmath.pi / 180, 0.1)
        time.sleep(2)

    motionProxy.setAngles("RShoulderRoll", -70.0 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderPitch", 80 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -17 * cmath.pi / 180, 0.1)
    time.sleep(3)
    pass


def putCylinderDown():
    # 放下水果
    motionProxy.setAngles("RShoulderRoll", -70 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderPitch", -10 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 3 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RShoulderRoll", 17 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 50 * cmath.pi / 180, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RHand", 1, 0.1)
    time.sleep(3)

    motionProxy.setAngles("RElbowRoll", 2.0 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -70 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderPitch", 80 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -17 * cmath.pi / 180, 0.1)
    time.sleep(3)

    pass


if __name__ == '__main__':
    # 步态
    maxstepx = 0.02
    maxstepy = 0.03
    maxsteptheta = 0.1
    maxstepfrequency = 0.2
    maxstepheight = 0.05
    torsowx = 0.0
    torsowy = 0.0

    moveConfig = [["MaxStepX", maxstepx],
                  ["MaxStepY", maxstepy],
                  ["MaxStepTheta", maxsteptheta],
                  ["StepHeight", maxstepheight],
                  ["MaxStepFrequency", maxstepfrequency],
                  ["TorsoWx", torsowx],
                  ["TorsoWy", torsowy]]

    motionProxy = ALProxy("ALMotion", IP, PORT)
    ttsProxy = ALProxy("ALTextToSpeech", IP, PORT)
    postureProxy = ALProxy("ALRobotPosture", IP, PORT)
    memorypxy = ALProxy("ALMemory", IP, PORT)
    motionProxy.wakeUp()

    # 站直
    standUp()

    # 抓取水果
    catchTheCylinder()

    # 洗水果
    washFruit()

    # 放下小球
    putCylinderDown()